
#include "rviz_custom_plugins/panels/flyzone_panel.h"
#include <QStandardItemModel>
#include <QStringBuilder>
#include "rviz_custom_plugins/utils/interactive_marker_manager.h"
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/parser.h"
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include "rviz_custom_plugins/utils/interactive_marker_factory.h"
#include "rviz_custom_plugins/models/flyzones.h"
#include "rviz_custom_plugins/utils/project_manager.h"
#include <QDir>
#include <QFile>
#include <QTextStream>
#include <nlohmann/json.hpp>


using nlohmann::json;



using rviz_custom_plugins::utils::InteractiveMarkerManager;
using rviz_custom_plugins::utils::parseColor;
using rviz_custom_plugins::utils::parsePoint;

using rviz_custom_plugins::utils::getMetadataItem;
using rviz_custom_plugins::utils::InteractiveMarkerFactory;

using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;

using rviz_custom_plugins::models::Flyzones;
using rviz_custom_plugins::models::Flyzone;
using rviz_custom_plugins::utils::ProjectManager;



namespace rviz_custom_plugins {


namespace panels {


FlyzonePanel::FlyzonePanel(QWidget *parent) : rviz_common::Panel(parent), ui(new Ui::flyzonePanel())
{
    ui->setupUi(this);

    ProjectManager* projectManager = ProjectManager::getInstance();

    treeModel = new PointGroupModel();
    ui->treeView->setFocusPolicy(Qt::StrongFocus);
    ui->treeView->header()->hide();
    ui->treeView->setModel(treeModel);
    ui->treeView->setColumnWidth(0, 100);
    ui->treeView->setIndentation(15);
    // ui->treeView->setColumnHidden(3, true);
    // 设置后续列自动拉伸（从第1列开始到最后）
    ui->treeView->header()->setSectionResizeMode(1, QHeaderView::Stretch);
    ui->treeView->header()->setSectionResizeMode(2, QHeaderView::Stretch);
    markerColor = QColor(255, 0, 0, 64);
    initialPosition.x = 0.0f;
    initialPosition.y = 0.0f;
    initialPosition.z = 0.0f;

    connect(ui->btnAddBox, &QPushButton::clicked, this, &FlyzonePanel::addBoxButtonClicked);
    connect(ui->btnRemoveBox, &QPushButton::clicked, this, &FlyzonePanel::removeBoxButtonClicked);

    ui->btnAddPoint->setVisible(false);
    ui->btnRemovePoint->setVisible(false);
    ui->btnInsertPoint->setVisible(false);
    // connect(ui->btnAddPoint, &QPushButton::clicked, this, &FlyzonePanel::addPointButtonClicked);
	// connect(ui->btnRemovePoint, &QPushButton::clicked, this, &FlyzonePanel::removePointButtonClicked);
	// connect(ui->btnInsertPoint, &QPushButton::clicked, this, &FlyzonePanel::insertPointButtonClicked);

    connect(treeModel, &PointGroupModel::itemOrderUpdated, this, &FlyzonePanel::handlerPointsReorder, Qt::DirectConnection);
    // connect(treeModel, &PointGroupModel::boxOrderUpdated, this, &FlyzonePanel::handlerBoxesReorder, Qt::DirectConnection);
    // connect(this, &FlyzonePanel::markerFeedbackReceived,  this, &FlyzonePanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(projectManager, &ProjectManager::projectCreated, this, &FlyzonePanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectOpened, this, &FlyzonePanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectSaved, this, &FlyzonePanel::handleProjectSaved, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectClosed, this, &FlyzonePanel::handleProjectClosed, Qt::DirectConnection);
    
}


void FlyzonePanel::load(const Config& config)
{
    Panel::load(config);
    // set marker server namespace
    QString nameSpace;
    config.mapGetString("NameSpace", &nameSpace);
	markerThread = InteractiveMarkerManager::getInstance()->addThread(nameSpace);
    connect(markerThread.get(), &InteractiveMarkerThread::interactiveMarkerFeedback, this, &FlyzonePanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(markerThread.get(), &InteractiveMarkerThread::activeStatusChanged, this, &FlyzonePanel::handleActiveStatusChanged, Qt::QueuedConnection);
    handleActiveStatusChanged(markerThread->getActive());

    // set zone type
    config.mapGetInt("ZoneType", &zoneType);

    // set groupbox title
    QString groupTitle;
    config.mapGetString("GroupTitle", &groupTitle);
    if (!groupTitle.isEmpty()) {
        treeModel->setGroupTitlePrefix(groupTitle);
        // ui->groupBox->setTitle(groupTitle);
    }

    // set initial point
    QString initialPositionStr;
    config.mapGetString("InitialPosition", &initialPositionStr);
    if(!initialPositionStr.isEmpty()) {
        initialPosition = parsePoint(initialPositionStr);
    }
    // set marker color
    QString markerColorStr;
    config.mapGetString("MarkerColor", &markerColorStr);
    if(!markerColorStr.isEmpty()) {
        markerColor = parseColor(markerColorStr);
    }
}


void FlyzonePanel::addBoxButtonClicked()
{
    geometry_msgs::msg::Point maxPosition;
    maxPosition.x = 1.0f + initialPosition.x;
    maxPosition.y = 1.0f + initialPosition.y;
    maxPosition.z = 1.0f + initialPosition.z;

    addGroup(initialPosition, maxPosition);
}


void FlyzonePanel::getCurrentIndex(int& l1Row, int& l2Row)
{
    l1Row = -1;
    l2Row = -1;
    QModelIndex currentIndex = ui->treeView->selectionModel()->currentIndex();
    if(currentIndex.isValid()) {
        // 父节点索引
        QModelIndex parentIndex = currentIndex.parent();
        // 有父节点则说明本节点是二级节点
        if (parentIndex.isValid()) {
            l1Row = parentIndex.row();
            l2Row = currentIndex.row();
        }
        // 没有父节点则说明本节点是一级节点
        else {
            l1Row = currentIndex.row();
        }
    }
}

void FlyzonePanel::removeBoxButtonClicked()
{
    int l1Row = -1;
    int l2Row = -1;
    getCurrentIndex(l1Row, l2Row);
    if(l1Row != -1) {
        removeGroup(l1Row);
    }
}


void FlyzonePanel::addGroup(geometry_msgs::msg::Point minPoint, geometry_msgs::msg::Point maxPoint)
{
    treeModel->appendGroup();
    int lastRow = treeModel->rowCount() - 1;
    // 获取最后一行的索引
    QModelIndex lastIndex = treeModel->index(lastRow, 0); 
    ui->treeView->selectionModel()->select(lastIndex, QItemSelectionModel::Select);
    ui->treeView->setCurrentIndex(lastIndex);
    // 添加最小点
    InteractiveMarker minIntMarker = InteractiveMarkerFactory::createFlyzonePoint(markerColor, minPoint);
    markerThread->insert(minIntMarker);
    auto markers = treeModel->getGroupItems(lastRow);
    treeModel->insertGroupItem(lastRow, markers.size(), minIntMarker);
    // 添加最大点
    InteractiveMarker maxIntMarker = InteractiveMarkerFactory::createFlyzonePoint(markerColor, maxPoint);
    markerThread->insert(maxIntMarker);
    treeModel->insertGroupItem(lastRow, markers.size(), maxIntMarker);   
}


void FlyzonePanel::removeGroup(int l1Row)
{
    // 删除box中的所有点
    std::list<InteractiveMarker>& markers = treeModel->getGroupItems(l1Row);
    for(auto& marker: markers) {
        markerThread->erase(marker.name);
    }

    // 删除box
    std::string signName = treeModel->getGroupSign(l1Row);
    markerThread->erase(signName);

    // 删除model
    treeModel->removeGroup(l1Row);
}


// void FlyzonePanel::addPointButtonClicked()
// {
//     int l1Row = -1;
//     int l2Row = -1;
//     getCurrentIndex(l1Row, l2Row);
//     if(l1Row != -1) {

//         InteractiveMarker intMarker = InteractiveMarkerFactory::createFlyzonePoint(markerColor, initialPosition);

//         markerThread->insert(intMarker);

//         auto markers = treeModel->getGroupItems(l1Row);
//         treeModel->insertGroupItem(l1Row, markers.size(), intMarker);
//     }
// }

// void FlyzonePanel::removePointButtonClicked()
// {
// 	int l1Row = -1;
//     int l2Row = -1;
//     getCurrentIndex(l1Row, l2Row);
// 	if(l1Row != -1 && l2Row != -1)
// 	{
//         InteractiveMarker& marker = treeModel->getGroupItem(l1Row, l2Row);
//         markerThread->erase(marker.name);

//         treeModel->removeGroupItem(l1Row, l2Row);
// 	}
// }


// void FlyzonePanel::insertPointButtonClicked() {

// 	int l1Row = -1;
//     int l2Row = -1;
//     getCurrentIndex(l1Row, l2Row);
//     if(l1Row != -1 && l2Row != -1) {
//         InteractiveMarker intMarker = InteractiveMarkerFactory::createFlyzonePoint(markerColor, initialPosition);

//         markerThread->insert(intMarker);
//         treeModel->insertGroupItem(l1Row, l2Row, intMarker);
//     }
// }


void FlyzonePanel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {

    QString markerName = QString::fromStdString(feedback->marker_name);
    if(markerName.startsWith("flyzone_polygon_", Qt::CaseSensitive))
    {
        return;
    }
    else if(markerName.startsWith("flyzone_point_", Qt::CaseSensitive))
    {
        if(MARKER_ADDED == feedback->client_id)
        {
            // visualization_msgs::msg::InteractiveMarker intMarker;
            // markerThread->get(feedback->marker_name, intMarker);

            std::ostringstream sstm;
            sstm << "add marker marker_name:" << feedback->marker_name;
            // sstm << " marker_l1_row:" << l1Row;
            // sstm << " marker_l2_row:" << l2Row;
            RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
            return;
        }
        else if(MARKER_UPDATED == feedback->client_id)
        {
            // visualization_msgs::msg::InteractiveMarker intMarker;
            // markerThread->get(feedback->marker_name, intMarker);

            std::ostringstream sstm;
            sstm << "update marker marker_name:" << feedback->marker_name;
            // sstm << " marker_l1_row:" << l1Row;
            // sstm << " marker_l2_row:" << l2Row;
            RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
            return;
        }
        else if(MARKER_REMOVED == feedback->client_id)
        {
            // int l1Row = -1;
            // int l2Row = -1;
            // treeModel->getMarkerRow(markerName, l1Row, l2Row);

            std::ostringstream sstm;
            sstm << "remove marker marker_name:" << feedback->marker_name;
            // sstm << " marker_l1_row:" << l1Row;
            // sstm << " marker_l2_row:" << l2Row;
            RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

            return;
        }

        switch (feedback->event_type) {
            case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE:
            {
                int l1Row = -1;
                int l2Row = -1;
                treeModel->getMarkerRow(markerName, l1Row, l2Row);
                if (l1Row != -1 && l2Row != -1)
                {
                    QModelIndex zoneIndex = treeModel->index(l1Row, 0);
                    // 获取二级节点的索引（假设第1行是二级节点）
                    QModelIndex pointIndex = treeModel->index(l2Row, 0, zoneIndex);
                    ui->treeView->setCurrentIndex(pointIndex);
                    // 修改点的坐标
                    treeModel->handleMarkerFeedback(feedback);

                    // 重新画出多边形
                    std::list<InteractiveMarker>& points = treeModel->getGroupItems(l1Row);
                    if(points.size() == 2) {
                        std::string signName = treeModel->getGroupSign(l1Row);
                        InteractiveMarker signMarker = InteractiveMarkerFactory::createCube(signName, markerColor, points);
                        markerThread->insert(signMarker);
                    }
                    
                }
                break;
            }
        }
    }
}

void FlyzonePanel::handleActiveStatusChanged(bool active) {
    if (active) {
        setEnabled(true);
        parentWidget()->show();
    } else {
        setEnabled(false);
        parentWidget()->hide();
    }
}


void FlyzonePanel::handlerPointsReorder(int l1Row, int l2Row, std::list<InteractiveMarker>& markers) {
    auto it = markers.begin();
    std::advance(it, l2Row); // 移动到指定索引

    for ( ;it != markers.end();++it) {
        int markerShowRow = getMetadataItem(*it, PointGroupModel::L2_ROW, "-1").toInt() + 1;
        it->description = std::to_string(markerShowRow);
        markerThread->insert(*it);
    }

    std::string signName = treeModel->getGroupSign(l1Row);
    if(markers.size() == 2) {
        InteractiveMarker signMarker = InteractiveMarkerFactory::createCube(signName, markerColor, markers);
        markerThread->insert(signMarker);
    }
    else {
        markerThread->erase(signName);
    }
}


void FlyzonePanel::handleProjectLoaded(QSharedPointer<QVariantMap> projectData)
{
    QVariant flyzonesVar; 
    if(zoneType == 1) {
        flyzonesVar = (*projectData)["safeAreas"];
    } else {
        flyzonesVar = (*projectData)["limitAreas"];
    }
    
    Flyzones flyzones = flyzonesVar.value<Flyzones>();
    for (const auto& zone : flyzones.flyzones) {
        addGroup(zone.minPoint, zone.maxPoint);
    }
    
}


void FlyzonePanel::handleProjectSaved(QSharedPointer<QVariantMap> projectData)
{
    auto markersList = treeModel->getGroups();
    // std::ostringstream sstm;
    // sstm << " markersList size:" << markersList.size();
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    Flyzones flyzones;
    int index = 1;
    for (auto markersIt = markersList.begin(); markersIt != markersList.end(); markersIt++, ++index) 
    {
        Flyzone flyzone;
        flyzone.id = index;
        auto it = markersIt->begin();
        const auto& minPos = it->pose.position;
        flyzone.minPoint = minPos;       
        ++it;
        const auto& maxPos = it->pose.position;
        flyzone.maxPoint = maxPos;  
        flyzones.flyzones.push_back(flyzone);
    }

    if(zoneType == 1) {
        (*projectData)["safeAreas"] = QVariant::fromValue(flyzones);
    } else {
        (*projectData)["limitAreas"] = QVariant::fromValue(flyzones);
    }
    
}


void FlyzonePanel::handleProjectClosed(QSharedPointer<QVariantMap> projectData)
{
    int lastRow = treeModel->rowCount() - 1;
    for(;lastRow >= 0; lastRow--)
    {
        removeGroup(lastRow);
    }
} 


}


}


#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::panels::FlyzonePanel, rviz_common::Panel )
